#include "Includes.h"
#include "eeprom.h"

#pragma region //============字段=============
#define PC_BUF_SIZE		200
static uint8_t RxBuf[PC_BUF_SIZE];
static uint8_t FrBuf[PC_BUF_SIZE];
static uint8_t TxBuf[PC_BUF_SIZE];
static UartDriver * PCObj = NULL;
const uint16_t DefaultMotorDir = 0;
const uint8_t DefaultCtrlLoop[] = {
	POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP,
	POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP,
	POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP, POS_SPD_LOOP,
};
const set_pos_t DefaultPos[] = { 
	/*收起位*//*中位*//*放下位*/
	{0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191},
	{0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191},
	{0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191}, {0, 4095, 8191},		
};
const PID_t DefaultPosPIDs[] = { 
	/*Kp*//*Ki*//*Kd*//*积分上限*//*积分下限*/
	{200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024},
	{200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024},
	{200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024}, {200, 0, 0, 1024, -1024},
};
const PID_t DefaultSpdPIDs[] = { 
	{50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024},
	{50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024},
	{50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024}, {50, 0, 0, 1024, -1024},
};
const PID_t DefaultCurPIDs[] = { 
	{10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100},
	{10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100},
	{10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100}, {10, 0, 0, 100, -100},
};
#pragma endregion
#pragma region //============变量=============
extern osSemaphoreId RxPC_IdleSemHandle;
extern osSemaphoreId TxPC_CpltSemHandle;
extern osMessageQId PC_TxMsgQueueHandle;
extern int Angle[];
extern int WorkMode;
eeprom_var_t eeprom_var;
#pragma endregion

uint8_t CheckSum(uint8_t *pByte, uint8_t len)
{
	uint8_t chk_sum = 0;
	while (len-- > 0)
	{
		chk_sum += *pByte++;
	}
	return chk_sum;
}

#pragma region //===========私有函数=============
extern uint8_t NowMotor;
int PakeCtrlPosFr(uint8_t *pTxbuf)
{
	get_set_ctrl_pos_t *pt = (get_set_ctrl_pos_t*)pTxbuf;
	pt->sync = C2P_SYNC;
	pt->frame_type = C2P_CTRL_POS_fr;
	pt->motor = NowMotor;
	pt->motor_dir = eeprom_var.motor_dir;
	pt->pos.packup = eeprom_var.pos[NowMotor].packup;
	pt->pos.mid = eeprom_var.pos[NowMotor].mid;
	pt->pos.laydown = eeprom_var.pos[NowMotor].laydown;
	pt->check_sum = CheckSum(&pTxbuf[1], sizeof(get_set_ctrl_pos_t) - 2);
	return sizeof(get_set_ctrl_pos_t);
}
int PakePID_Fr(uint8_t *pTxbuf)
{
	get_set_ctrl_pid_t *pt = (get_set_ctrl_pid_t*)pTxbuf;
	pt->sync = C2P_SYNC;
	pt->frame_type = C2P_PID_DATA_fr;
	pt->motor = NowMotor;
	pt->pos_pid = eeprom_var.pos_pids[NowMotor];
	pt->spd_pid = eeprom_var.spd_pids[NowMotor];
	pt->cur_pid = eeprom_var.cur_pids[NowMotor];
	pt->check_sum = CheckSum(&pTxbuf[1], sizeof(get_set_ctrl_pid_t) - 2);
	return sizeof(get_set_ctrl_pid_t);
}
void DecodeSetCtrlPosFr(uint8_t *pRxBuf)
{
	get_set_ctrl_pos_t *pt = (get_set_ctrl_pos_t*)pRxBuf;
	NowMotor = pt->motor;
	eeprom_var.pos[NowMotor].packup = pt->pos.packup;
	eeprom_var.pos[NowMotor].mid = pt->pos.mid;
	eeprom_var.pos[NowMotor].laydown = pt->pos.laydown;
	osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
}
void DecodeSetPID_Fr(uint8_t *pRxBuf)
{
	get_set_ctrl_pid_t *pt = (get_set_ctrl_pid_t*)pRxBuf;
	NowMotor = pt->motor;
	eeprom_var.pos_pids[NowMotor] = pt->pos_pid;
	eeprom_var.spd_pids[NowMotor] = pt->spd_pid;
	eeprom_var.cur_pids[NowMotor] = pt->cur_pid;
	osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakePID_Fr, 0);
}
extern int pos_am;
extern int pos_tc;
extern int spd_am;
extern int spd_tc;
extern int cur_am;
extern int cur_tc;
void DecodeStartCalPID_Fr(unsigned char *pRxBuf)
{
	sStartCalPID_Fr *pt = (sStartCalPID_Fr*)pRxBuf;
	NowMotor = pt->motor;
	eeprom_var.ctrl_loop[NowMotor] = pt->ctrl_loop;
	pos_am = pt->angle_am;
	pos_tc = pt->angle_cycle;
	spd_am = pt->speed_am;
	spd_tc = pt->speed_cycle;
	cur_am = pt->currt_am;
	cur_tc = pt->currt_cycle;
	WorkMode = CALIBRATION_PID;
}
//描述: 解码通信协议
void DecodeComProtocol(unsigned char *pFrType/*[I]指向帧类型*/)
{
	uint8_t nowCmd = *pFrType--;
	switch (nowCmd)
	{
	case P2C_GET_CTRL_POS_fr: //获得控制位置
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
		break;
	case P2C_SET_CTRL_POS_fr:
		DecodeSetCtrlPosFr(pFrType);
		break;
	case P2C_CAP_MID_POS_fr:
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		eeprom_var.pos[NowMotor].mid = Angle[NowMotor];
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
		break;
	case P2C_CAP_PACKUP_POS_fr:
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		eeprom_var.pos[NowMotor].packup = Angle[NowMotor];
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
		break;
	case P2C_CAP_LAYDOWN_POS_fr:
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		eeprom_var.pos[NowMotor].laydown = Angle[NowMotor];
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
		break;
	case P2C_AUTO_MDIX_fr:
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		WorkMode = CALIBRATION_DIR;
		break;
	case P2C_GET_PID_fr: //获得pid数据
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakePID_Fr, 0);
		break;
	case P2C_SET_PID_fr:
		DecodeSetPID_Fr(pFrType);
		break;
	case P2C_START_CAL_PID_fr:
		DecodeStartCalPID_Fr(pFrType);
		break;
	case P2C_STOP_MOTOR_fr:
		NowMotor = ((cmd_frame_t*)pFrType)->para;
		eeprom_var.pos[NowMotor].laydown = Angle[NowMotor];
		osMessagePut(PC_TxMsgQueueHandle, (uint32_t)PakeCtrlPosFr, 0);
		break;
	case P2C_SAVE_EEPROM_VAR_fr:
		EE_SaveAllVar((uint16_t*)&eeprom_var);
		break;
	default	:
		break;
	}
}
/*
********************************************************************************
** 函数名称 :	GetFrameLen	获得帧长度
** 描    述 :	从给定的帧类型，得到帧长度(不包含sync)。如果帧类型不合法，返回0
** 返    回 :	返回帧长度。如果帧类型不合法，返回0
********************************************************************************/
//描述: 获得给定帧类型的帧长度, -1=不包含sync
//返回: 0:无效帧, 其它:帧长
static uint8_t GetFrameLen(uint8_t type/*[I]帧类型*/)
{
	switch (type)
	{
	case P2C_GET_CTRL_POS_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_SET_CTRL_POS_fr:
		return sizeof(get_set_ctrl_pos_t) - 1;
	case P2C_CAP_MID_POS_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_CAP_PACKUP_POS_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_CAP_LAYDOWN_POS_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_AUTO_MDIX_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_GET_PID_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_SET_PID_fr:
		return sizeof(get_set_ctrl_pid_t) - 1;
	case P2C_START_CAL_PID_fr:
		return sizeof(sStartCalPID_Fr) - 1;
	case P2C_STOP_MOTOR_fr:
		return sizeof(cmd_frame_t) - 1;
	case P2C_SAVE_EEPROM_VAR_fr:
		return sizeof(cmd_frame_t) - 1;
	default:
		return 0;
	}
}
//描述: 解码1帧数据.支持帧格式为：SYNC[1B]+帧类型[1B]+id+数据+...+校验和[1B]。
//帧长（不含同步字节）:根据帧类型可获得。
//校验和:不含同步字节，之前数据相加得到。
static void DecodeFrameData(uint8_t *pByte, uint8_t size)
{
	while (size-- >=  4)	//4为最短帧长
	{
		if (*pByte++ != P2C_SYNC)
			continue;								//寻找同步头
		uint8_t len = GetFrameLen(*pByte); //帧长-1
		if ((len == 0) || (len > size))
			continue;								//帧类型不合法
		if (CheckSum(pByte, len - 1) != pByte[len - 1])
			continue;								//校验和不正确
		//指向pByte指向帧类型，len = 帧大小-1，size = SIZE-1
		DecodeComProtocol(pByte);
		size -= len; //size-len = SIZE-1-(帧大小-1) = SIZE-帧SIZE => 去掉一帧的数据
		pByte += len; //pByte+len = 指向帧类型+(帧长-1) = 指向sync+帧长 => 指向下一帧头
	}
}
#pragma endregion

#pragma region //============方法==============
void RxDataFromPC_Task(void const * argument)
{
	UD_Register(&PCObj, &huart2, RxBuf, FrBuf, PC_BUF_SIZE);
	UD_Init(PCObj, TxPC_CpltSemHandle, RxPC_IdleSemHandle, osWaitForever, 3);
	
	/*从EEPROM中读取IP配置信息*/
	eeprom_var.motor_dir = DefaultMotorDir;
	for (size_t i = 0; i < 12; i++)
	{
		eeprom_var.ctrl_loop[i] = DefaultCtrlLoop[i];
		eeprom_var.pos[i] = DefaultPos[i];
		eeprom_var.pos_pids[i] = DefaultPosPIDs[i];
		eeprom_var.spd_pids[i] = DefaultSpdPIDs[i];
		eeprom_var.cur_pids[i] = DefaultCurPIDs[i];
	}
	int ret = EE_Init();
	if (ret == EE_OK)
	{
		ret = EE_LoadAllVar((uint16_t*)&eeprom_var);
		if (ret != EE_OK)
		{
			EE_SaveAllVar((uint16_t*)&eeprom_var); /*使用全部或部分的默认信息，存储到EEPROM中*/
		}
	}
	
	WorkMode = STANDBY_MODE;
	
	for (;;)
	{
		int len = UD_ReadFrameData(PCObj);
		//解码帧数据
		DecodeFrameData(FrBuf, len);
	}
}

void TxDataToPC_Task(void const * argument)
{
	while (PCObj == NULL)
	{
		osDelay(10);
	}
	
	for (;;)
	{
		osEvent event = osMessageGet(PC_TxMsgQueueHandle, osWaitForever);
		if (event.status == osEventMessage)
		{
			int(*fun)(uint8_t*) = (int(*)(uint8_t*))event.value.p;
			int len = fun(TxBuf);
			UD_Transmit(PCObj, TxBuf, len);
		}
	}
}
#pragma endregion
